/*****************************************************************************/
/*                                                                           */
/*    Domino Operation System Driver Module                                  */
/*                                                                           */
/*    Copyright (C) 2007 Laszlo Arvai                                        */
/*                                                                           */
/*    ------------------------------------------------------------------     */
/*    drvRCServo.c - R/C Servo pulse generator driver                        */
/*****************************************************************************/

///////////////////////////////////////////////////////////////////////////////
// Includes
#include <drvRCServo.h>
#include "sysSettings.h"

///////////////////////////////////////////////////////////////////////////////
// Local variables
dosWord l_channel_values[RCS_CHANEL_COUNT];

dosByte l_group1_channel = RCS_GROUP_CHANNEL_COUNT;
dosWord l_group1_length = 0;

#if RCS_CHANEL_COUNT > 8
dosByte l_group2_channel = RCS_GROUP_CHANNEL_COUNT;
dosWord l_group2_length = 0;
#endif

/*
RCServoPinMappingEntry l_pin_mapping[RCS_CHANEL_COUNT] = 
{
	{ (dosByte*)0x32, 1<<PD0 },
	{ (dosByte*)0x32, 1<<PD1 },
	{ (dosByte*)0x32, 1<<PD2 },
	{ (dosByte*)0x32, 1<<PD3 },
	{ (dosByte*)0x32, 1<<PD4 },
	{ (dosByte*)0x32, 1<<PD5 },
	{ (dosByte*)0x32, 1<<PD6 },
	{ (dosByte*)0x32, 1<<PD7 },
};

*/
///////////////////////////////////////////////////////////////////////////////
// Functions


///////////////////////////////////////////////////////////////////////////////
// Init RC sevo driver
void drvInitRCServo(void)
{
	dosByte i;

	for( i = 0; i < RCS_CHANEL_COUNT; i++ )
		l_channel_values[i] = RCS_MAX_VALUE / 2;
}

///////////////////////////////////////////////////////////////////////////////
// Set channel value
void drvRCSSetPosition( dosByte in_channel, dosWord in_value )
{
	if( in_channel <= RCS_CHANEL_COUNT && in_value <= RCS_MAX_VALUE )
		l_channel_values[in_channel] = in_value;
}

///////////////////////////////////////////////////////////////////////////////
// Interrupt callback function 
dosWord drvRCServoInterruptCallback1(void)
{
	dosWord delay_value = 0;

  // set channel to low and go to the next channel
	if( l_group1_channel >= RCS_CHANEL_COUNT || l_group1_channel >= RCS_GROUP_CHANNEL_COUNT )
	{
		// after processing all channels -> start with channel 0
		l_group1_channel = 0;
		l_group1_length = 0;
	}
	else
	{
		// processing channels
    drvRCServoSetLow( l_group1_channel );
		l_group1_channel++;
	}

  // set channel to high
	if( l_group1_channel < RCS_CHANEL_COUNT && l_group1_channel < RCS_GROUP_CHANNEL_COUNT )
	{
		drvRCServoSetHigh( l_group1_channel );

		delay_value = (dosWord)((dosDWord)(l_channel_values[l_group1_channel]) * (RCS_MAX_PULSE - RCS_MIN_PULSE) / RCS_MAX_VALUE + RCS_MIN_PULSE);

		l_group1_length += delay_value;
	}
	else
	{
		// after last channel -> pulse repeat delay
		delay_value = RCS_PULSE_REPEAT_RATE - l_group1_length;
	}

	return delay_value;
}

#if RCS_CHANEL_COUNT > 8
///////////////////////////////////////////////////////////////////////////////
// Interrupt callback function 
dosWord drvRCServoInterruptCallback2(void)
{
	dosWord delay_value = 0;

	if( l_group2_channel >= RCS_CHANEL_COUNT || l_group2_channel >= RCS_GROUP_CHANNEL_COUNT )
	{
		// after processing all channels -> start with channel 0
		l_group2_channel = 0;
		l_group2_length = 0;
	}
	else
	{
		// processing channels
		*l_pin_mapping[l_group2_channel+RCS_GROUP_CHANNEL_COUNT].PortAddress &= ~l_pin_mapping[l_group2_channel+RCS_GROUP_CHANNEL_COUNT].BitMask;
		l_group2_channel++;
	}

	if( l_group2_channel < RCS_CHANEL_COUNT && l_group2_channel < RCS_GROUP_CHANNEL_COUNT )
	{
		*l_pin_mapping[l_group2_channel+RCS_GROUP_CHANNEL_COUNT].PortAddress |= l_pin_mapping[l_group2_channel+RCS_GROUP_CHANNEL_COUNT].BitMask;

		delay_value = (dosWord)((dosDWord)(l_channel_values[l_group2_channel+RCS_GROUP_CHANNEL_COUNT]) * (RCS_MAX_PULSE - RCS_MIN_PULSE) / RCS_MAX_VALUE + RCS_MIN_PULSE);

		l_group2_length += delay_value;
	}
	else
	{
		// after last channel -> pulse repeat delay
		delay_value = RCS_PULSE_REPEAT_RATE - l_group2_length;
	}

	return delay_value;
}
#endif
